//
// Created by root on 20-8-9.
//

#include "ext_row.h"

#ifdef __cplusplus
namespace libyuv {
    extern "C" {
#endif

#define SS(width, shift) (((width) + (1 << (shift)) - 1) >> (shift))
// 16x2 pixels -> 8x1.  width is number of argb pixels. e.g. 16.
#define BGRTOUV(QB, QG, QR)                                                 \
  "vmul.s16   q8, " #QB ", q10               \n" /* B                    */ \
  "vmls.s16   q8, " #QG ", q11               \n" /* G                    */ \
  "vmls.s16   q8, " #QR ", q12               \n" /* R                    */ \
  "vadd.u16   q8, q8, q15                    \n" /* +128 -> unsigned     */ \
  "vmul.s16   q9, " #QR ", q10               \n" /* R                    */ \
  "vmls.s16   q9, " #QG ", q14               \n" /* G                    */ \
  "vmls.s16   q9, " #QB ", q13               \n" /* B                    */ \
  "vadd.u16   q9, q9, q15                    \n" /* +128 -> unsigned     */ \
  "vqshrn.u16  d0, q8, #8                    \n" /* 16 bit to 8 bit U    */ \
  "vqshrn.u16  d1, q9, #8                    \n" /* 16 bit to 8 bit V    */

// Any 1 to 1.
#define ANY11(NAMEANY, ANY_SIMD, UVSHIFT, SBPP, BPP, MASK)                \
  void NAMEANY(const uint8_t* src_ptr, uint8_t* dst_ptr, int width) {     \
    SIMD_ALIGNED(uint8_t temp[128 * 2]);                                  \
    memset(temp, 0, 128); /* for YUY2 and msan */                         \
    int r = width & MASK;                                                 \
    int n = width & ~MASK;                                                \
    if (n > 0) {                                                          \
      ANY_SIMD(src_ptr, dst_ptr, n);                                      \
    }                                                                     \
    memcpy(temp, src_ptr + (n >> UVSHIFT) * SBPP, SS(r, UVSHIFT) * SBPP); \
    ANY_SIMD(temp, temp + 128, MASK + 1);                                 \
    memcpy(dst_ptr + n * BPP, temp + 128, r * BPP);                       \
  }
    ANY11(BGR24ToYRow_Any_NEON, BGR24ToYRow_NEON, 0, 3, 1, 7)
#undef ANY11

// Any 1 to 2 with source stride (2 rows of source).  Outputs UV planes.
// 128 byte row allows for 32 avx BGR24 pixels.
#define ANY12S(NAMEANY, ANY_SIMD, UVSHIFT, BPP, MASK)                        \
  void NAMEANY(const uint8_t* src_ptr, int src_stride_ptr, uint8_t* dst_u,   \
               uint8_t* dst_v, int width) {                                  \
    SIMD_ALIGNED(uint8_t temp[128 * 4]);                                     \
    memset(temp, 0, 128 * 2); /* for msan */                                 \
    int r = width & MASK;                                                    \
    int n = width & ~MASK;                                                   \
    if (n > 0) {                                                             \
      ANY_SIMD(src_ptr, src_stride_ptr, dst_u, dst_v, n);                    \
    }                                                                        \
    memcpy(temp, src_ptr + (n >> UVSHIFT) * BPP, SS(r, UVSHIFT) * BPP);      \
    memcpy(temp + 128, src_ptr + src_stride_ptr + (n >> UVSHIFT) * BPP,      \
           SS(r, UVSHIFT) * BPP);                                            \
    if ((width & 1) && UVSHIFT == 0) { /* repeat last pixel for subsample */ \
      memcpy(temp + SS(r, UVSHIFT) * BPP, temp + SS(r, UVSHIFT) * BPP - BPP, \
             BPP);                                                           \
      memcpy(temp + 128 + SS(r, UVSHIFT) * BPP,                              \
             temp + 128 + SS(r, UVSHIFT) * BPP - BPP, BPP);                  \
    }                                                                        \
    ANY_SIMD(temp, 128, temp + 256, temp + 384, MASK + 1);                   \
    memcpy(dst_u + (n >> 1), temp + 256, SS(r, 1));                          \
    memcpy(dst_v + (n >> 1), temp + 384, SS(r, 1));                          \
  }
    ANY12S(BGR24ToUVRow_Any_NEON, BGR24ToUVRow_NEON, 0, 3, 15)
#undef ANY12S

// ARGBToY_C and ARGBToUV_C
#define MAKEROWY(NAME, R, G, B, BPP)                                         \
  void NAME##ToYRow_C(const uint8_t* src_argb0, uint8_t* dst_y, int width) { \
    int x;                                                                   \
    for (x = 0; x < width; ++x) {                                            \
      dst_y[0] = RGBToY(src_argb0[R], src_argb0[G], src_argb0[B]);           \
      src_argb0 += BPP;                                                      \
      dst_y += 1;                                                            \
    }                                                                        \
  }                                                                          \
  void NAME##ToUVRow_C(const uint8_t* src_rgb0, int src_stride_rgb,          \
                       uint8_t* dst_u, uint8_t* dst_v, int width) {          \
    const uint8_t* src_rgb1 = src_rgb0 + src_stride_rgb;                     \
    int x;                                                                   \
    for (x = 0; x < width - 1; x += 2) {                                     \
      uint8_t ab = (src_rgb0[B] + src_rgb0[B + BPP] + src_rgb1[B] +          \
                    src_rgb1[B + BPP]) >>                                    \
                   2;                                                        \
      uint8_t ag = (src_rgb0[G] + src_rgb0[G + BPP] + src_rgb1[G] +          \
                    src_rgb1[G + BPP]) >>                                    \
                   2;                                                        \
      uint8_t ar = (src_rgb0[R] + src_rgb0[R + BPP] + src_rgb1[R] +          \
                    src_rgb1[R + BPP]) >>                                    \
                   2;                                                        \
      dst_u[0] = RGBToU(ar, ag, ab);                                         \
      dst_v[0] = RGBToV(ar, ag, ab);                                         \
      src_rgb0 += BPP * 2;                                                   \
      src_rgb1 += BPP * 2;                                                   \
      dst_u += 1;                                                            \
      dst_v += 1;                                                            \
    }                                                                        \
    if (width & 1) {                                                         \
      uint8_t ab = (src_rgb0[B] + src_rgb1[B]) >> 1;                         \
      uint8_t ag = (src_rgb0[G] + src_rgb1[G]) >> 1;                         \
      uint8_t ar = (src_rgb0[R] + src_rgb1[R]) >> 1;                         \
      dst_u[0] = RGBToU(ar, ag, ab);                                         \
      dst_v[0] = RGBToV(ar, ag, ab);                                         \
    }                                                                        \
  }

    MAKEROWY(BGR24, 0, 1, 2, 3)
#undef MAKEROWY

    void BGR24ToYRow_NEON(const uint8_t *src_bgr, uint8_t *dst_y, int width) {
        asm volatile(
        "vmov.u8    d4, #25                        \n"  // B * 0.1016 coefficient
        "vmov.u8    d5, #129                       \n"  // G * 0.5078 coefficient
        "vmov.u8    d6, #66                        \n"  // R * 0.2578 coefficient
        "vmov.u8    d7, #16                        \n"  // Add 16 constant
        "1:                                        \n"
        "vld3.8     {d0, d1, d2}, [%0]!            \n"  // load 8 pixels of BGR24.
        "subs       %2, %2, #8                     \n"  // 8 processed per loop.
        "vmull.u8   q8, d0, d6                     \n"  // R
        "vmlal.u8   q8, d1, d5                     \n"  // G
        "vmlal.u8   q8, d2, d4                     \n"  // B
        "vqrshrn.u16 d0, q8, #8                    \n"  // 16 bit to 8 bit Y
        "vqadd.u8   d0, d7                         \n"
        "vst1.8     {d0}, [%1]!                    \n"  // store 8 pixels Y.
        "bgt        1b                             \n"
        : "+r"(src_bgr),  // %0
        "+r"(dst_y),      // %1
        "+r"(width)       // %2
        :
        : "cc", "memory", "d0", "d1", "d2", "d3", "d4", "d5", "d6", "d7", "q8");
    }

    void BGR24ToUVRow_NEON(const uint8_t *src_bgr,
                           int src_stride_bgr,
                           uint8_t *dst_u,
                           uint8_t *dst_v,
                           int width) {
        asm volatile (
        "add        %1, %0, %1                     \n"  // src_stride + src_bgr
        "vmov.s16   q10, #112 / 2                  \n"  // UB / VR 0.875 coefficient
        "vmov.s16   q11, #74 / 2                   \n"  // UG -0.5781 coefficient
        "vmov.s16   q12, #38 / 2                   \n"  // UR -0.2969 coefficient
        "vmov.s16   q13, #18 / 2                   \n"  // VB -0.1406 coefficient
        "vmov.s16   q14, #94 / 2                   \n"  // VG -0.7344 coefficient
        "vmov.u16   q15, #0x8080                   \n"  // 128.5
        "1:                                        \n"
        "vld3.8     {d0, d2, d4}, [%0]!            \n"  // load 8 BGR24 pixels.
        "vld3.8     {d1, d3, d5}, [%0]!            \n"  // load next 8 BGR24 pixels.
        "vpaddl.u8  q0, q0                         \n"  // R 16 bytes -> 8 shorts.
        "vpaddl.u8  q1, q1                         \n"  // G 16 bytes -> 8 shorts.
        "vpaddl.u8  q2, q2                         \n"  // B 16 bytes -> 8 shorts.
        "vld3.8     {d8, d10, d12}, [%1]!          \n"  // load 8 more BGR24 pixels.
        "vld3.8     {d9, d11, d13}, [%1]!          \n"  // load last 8 BGR24 pixels.
        "vpadal.u8  q0, q4                         \n"  // R 16 bytes -> 8 shorts.
        "vpadal.u8  q1, q5                         \n"  // G 16 bytes -> 8 shorts.
        "vpadal.u8  q2, q6                         \n"  // B 16 bytes -> 8 shorts.

        "vrshr.u16  q0, q0, #1                     \n"  // 2x average
        "vrshr.u16  q1, q1, #1                     \n"
        "vrshr.u16  q2, q2, #1                     \n"

        "subs       %4, %4, #16                    \n"  // 32 processed per loop.
        BGRTOUV(q2, q1, q0)
        "vst1.8     {d0}, [%2]!                    \n"  // store 8 pixels U.
        "vst1.8     {d1}, [%3]!                    \n"  // store 8 pixels V.
        "bgt        1b                             \n"
        : "+r"(src_bgr),  // %0
        "+r"(src_stride_bgr),  // %1
        "+r"(dst_u),     // %2
        "+r"(dst_v),     // %3
        "+r"(width)        // %4
        :
        : "cc", "memory", "q0", "q1", "q2", "q3", "q4", "q5", "q6", "q7",
        "q8", "q9", "q10", "q11", "q12", "q13", "q14", "q15"
        );
    }

#ifdef __cplusplus
    }  // extern "C"
}  // namespace libyuv
#endif